﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;
using System.Drawing.Drawing2D; // InterpolationMode

namespace ImageProcessingLibrary
{

    public unsafe class ImageProcessor
    {
        #region Fields
        private Bitmap processedBitmap;
        private BitmapData processedData;
        private Boolean isLocked;
        private Boolean isGrayScale;
        private Boolean isBinarized;
        private Boolean isComponentsConnected;
        private int[,] integralImage;

        private int[,] connectedComponentsLabels;
        private int sizeOfLargestComponent;
        private int labelOfLargestComponent;
        private int middleOfLargestComponent;
        private int pixelMiddleOfLargestComponent;
        #endregion

        #region Constants
        const int PIXEL_SIZE = 3;
        const int DEFAULT_THRESHOLD1 = 127;
        const int DEFAULT_THRESHOLD2 = 255;
        const int RUNNING_AVERAGE_BETA = 2;
        #endregion

        #region Private methods
        #endregion

        #region Public methods
        public ImageProcessor(Bitmap bitmap)
        {
            processedBitmap = new Bitmap(bitmap);
            isLocked = false;
            isGrayScale = false;
            isBinarized = false;
            isComponentsConnected = false;
        }

        public void Lock()
        {
            if (!isLocked)
            {
               processedData = processedBitmap.LockBits(
                       new Rectangle(0, 0, processedBitmap.Width, processedBitmap.Height),
                       ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb); //Changed to ReadWrite for multiple operations..
               isLocked = true;
            }
        }

        public void UnLock()
        {
            if (isLocked)
            {
                processedBitmap.UnlockBits(processedData);
                isLocked = false;
            }
        }

        public ImageProcessor Copy()
        {
            ImageProcessor copy = new ImageProcessor(this.ProcessedBitmap); // The constructor of ImageProcessor will copy the bitmap
            return copy;
        }

        public void Invert()
        {
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 0; j < processedData.Width; j++)
                {
                    byte rColor = (byte)(pRow[j * PIXEL_SIZE + 2]);
                    byte gColor = (byte)(pRow[j * PIXEL_SIZE + 1]);
                    byte bColor = (byte)(pRow[j * PIXEL_SIZE]);
                    byte maxValue = 255;
                    pRow[j * PIXEL_SIZE + 2] = (byte)(maxValue - rColor);
                    pRow[j * PIXEL_SIZE + 1] = (byte)(maxValue - gColor);
                    pRow[j * PIXEL_SIZE] = (byte)(maxValue - gColor);
                }
            }
        }

        public void ConvertToGrayScale(double rFraction, double gFraction, double bFraction)
        {
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 0; j < processedData.Width; j++)
                {
                    byte grayColor =
                      (byte)((pRow[j * PIXEL_SIZE] * bFraction) +
                             (pRow[j * PIXEL_SIZE + 1] * gFraction) +
                             (pRow[j * PIXEL_SIZE + 2] * rFraction));
                    pRow[j * PIXEL_SIZE] = grayColor;     //Note! B
                    pRow[j * PIXEL_SIZE + 1] = grayColor; //Note! G
                    pRow[j * PIXEL_SIZE + 2] = grayColor; //Note! R
                }
            }
            isGrayScale = true;
        }

        public void ConvertToStandardGrayScale()
        {
            ConvertToGrayScale(0.30, 0.59, 0.11);
        }

        public void BinarizeGray(int threshold1, int threshold2)
        {
            if (!isGrayScale)
            {
                ConvertToStandardGrayScale();
            }
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 0; j < processedData.Width; j++)
                {
                    byte pixelColor = (byte)(pRow[j * PIXEL_SIZE]);  // Any channel OK, since they are all equal..
                    if ((pixelColor < threshold1) || (pixelColor > threshold2))
                    {
                        pixelColor = 0;
                    }
                    else
                    {
                        pixelColor = 255;
                    }
                    pRow[j * PIXEL_SIZE] = pixelColor;
                    pRow[j * PIXEL_SIZE + 1] = pixelColor;
                    pRow[j * PIXEL_SIZE + 2] = pixelColor;
                }
            }
            isBinarized = true;
        }

        public void GenerateIntegralImage()
        {
            // Note: This method assumes that the image has been binarized.
            integralImage = new int[processedBitmap.Height, processedBitmap.Width];
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 0; j < processedBitmap.Width; j++)
                {
                    byte color = pRow[j * PIXEL_SIZE];
                    if (color == 0)
                    {
                        integralImage[i, j] = 0;
                    }
                    else
                    {
                        integralImage[i, j] = 1;
                    }
                    if (i > 0)
                    {
                        integralImage[i, j] += integralImage[i - 1, j];
                    }
                    if (j > 0)
                    {
                        integralImage[i, j] += integralImage[i, j - 1];
                    }
                    if ((i > 0) && (j > 0))
                    {
                        integralImage[i, j] -= integralImage[i - 1, j - 1];
                    }
                }
            }
        }

        public void RemoveSinglePixels()
        {
            // Assumes that the image has been binarized. 
            // If not, the image is binarized using default thresholds.
            if (!isBinarized)
            {
                BinarizeGray(DEFAULT_THRESHOLD1, DEFAULT_THRESHOLD2);
            }
            Bitmap cleanedBitmap = new Bitmap(processedBitmap.Width, processedBitmap.Height);
            BitmapData cleanedData = cleanedBitmap.LockBits(
               new Rectangle(0, 0, processedBitmap.Width, processedBitmap.Height),
               ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            for (int i = 1; i < processedBitmap.Height - 1; i++)  // Should perhaps introduce "EdgeIgnoreSize"
            {
                byte* pRowAbove = (byte*)processedData.Scan0 + ((i - 1) * processedData.Stride);
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                byte* pRowBelow = (byte*)processedData.Scan0 + ((i + 1) * processedData.Stride);
                byte* cRow = (byte*)cleanedData.Scan0 + (i * cleanedData.Stride);
                for (int j = 1; j < processedBitmap.Width - 1; j++)
                {
                    cRow[j * PIXEL_SIZE] = pRow[j * PIXEL_SIZE];
                    if (pRow[j * PIXEL_SIZE] != 0)
                    {
                        byte c11 = pRowAbove[(j - 1) * PIXEL_SIZE];
                        byte c12 = pRowAbove[j * PIXEL_SIZE];
                        byte c13 = pRowAbove[(j + 1) * PIXEL_SIZE];
                        if ((c11 == 0) && (c12 == 0) && (c13 == 0))
                        {
                            byte c21 = pRow[(j - 1) * PIXEL_SIZE];
                            byte c23 = pRow[(j + 1) * PIXEL_SIZE];
                            if ((c21 == 0) && (c23 == 0))
                            {
                                byte c31 = pRowBelow[(j - 1) * PIXEL_SIZE];
                                byte c32 = pRowBelow[j * PIXEL_SIZE];
                                byte c33 = pRowBelow[(j + 1) * PIXEL_SIZE];
                                if ((c31 == 0) && (c32 == 0) && (c33 == 0))
                                {
                                    cRow[j * PIXEL_SIZE] = 0;
                                }
                            }
                        }
                    }
                }
            }
            for (int i = 1; i < processedBitmap.Height - 1; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                byte* cRow = (byte*)cleanedData.Scan0 + (i * cleanedData.Stride);
                for (int j = 1; j < processedBitmap.Width - 1; j++)
                {
                    pRow[j * PIXEL_SIZE] = cRow[j * PIXEL_SIZE];
                    pRow[j * PIXEL_SIZE + 1] = cRow[j * PIXEL_SIZE];
                    pRow[j * PIXEL_SIZE + 2] = cRow[j * PIXEL_SIZE];
                }
            }
            cleanedBitmap.UnlockBits(cleanedData);
        }

        public Histogram GenerateHistogram(int channelIndex)
        {
            Histogram histogram = new Histogram(256);
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 0; j < processedData.Width; j++)
                {
                        byte pixelColor = (byte)(pRow[j * PIXEL_SIZE + (3 - channelIndex)]); // 1 = B, 2 = G, 3 = R.
                        histogram[pixelColor] += 1;
                }
            }
            return histogram;
        }

        public void EqualizeGrayHistogram() 
        {
            // Assumes that the image has been converted to gray scale.
            // If not, the image is converted to standard gray scale.
            if (!isGrayScale)
            {
                ConvertToStandardGrayScale();
            }
            Histogram histogram = GenerateHistogram(1); // Arbitrary channel..
            histogram.MakeCumulative();
            for (int i = 0; i < processedData.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                int width = processedBitmap.Width;
                int height = processedBitmap.Height;
                for (int j = 0; j < processedData.Width; j++)
                {
                    int grayColor = pRow[j * PIXEL_SIZE];
                    byte newColor = (byte)Math.Round(255.0 * histogram[grayColor] / (width * height));
                    pRow[j * PIXEL_SIZE] = newColor;     //Note! B
                    pRow[j * PIXEL_SIZE + 1] = newColor;
                    pRow[j * PIXEL_SIZE + 2] = newColor;
                }
            }
        }
       
        public void ConvolveGray(List<List<double>> convolutionMatrix)
        {
            if (!isGrayScale)
            {
                ConvertToStandardGrayScale();
            }
            Bitmap convolvedBitmap = new Bitmap(processedBitmap.Width, processedBitmap.Height);
            BitmapData convolvedData = convolvedBitmap.LockBits(
               new Rectangle(0, 0, processedBitmap.Width, processedBitmap.Height),
               ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            int halfSizeX = (int)Math.Truncate((convolutionMatrix[0].Count - 1) / 2.0);
            int halfSizeY = (int)Math.Truncate((convolutionMatrix.Count - 1) / 2.0);
            for (int i = halfSizeY; i < processedData.Height - halfSizeY; i++)
            {
                for (int j = halfSizeX; j < processedData.Width - halfSizeX; j++)
                {
                    // Compute the modified value using matrix multiplication:
                    double tmpConvolvedValue = 0;
                    for (int k = i - halfSizeY; k <= i + halfSizeY; k++)
                    {
                        int kCount = k - i + halfSizeY; // y-index of convolution matrix element.
                        byte* pRow = (byte*)processedData.Scan0 + k * processedData.Stride;
                        for (int m = j - halfSizeX; m <= j + halfSizeX; m++)
                        {
                            int mCount = m - j + halfSizeX; // x-index of convolution matrix element.
                            tmpConvolvedValue +=
                                pRow[m * PIXEL_SIZE] * convolutionMatrix[kCount][mCount]; // CHECK!!!
                        }
                    }
                    byte convolvedValue = (byte)Math.Round(tmpConvolvedValue);
                    if (tmpConvolvedValue < 0)
                    {
                        convolvedValue = 0;
                    }
                    else if (tmpConvolvedValue > 255)
                    {
                        convolvedValue = 255;
                    }
                    byte* cRow = (byte*)convolvedData.Scan0 + i * convolvedData.Stride;
                    cRow[j * PIXEL_SIZE] = convolvedValue;
                    cRow[j * PIXEL_SIZE + 1] = convolvedValue;
                    cRow[j * PIXEL_SIZE + 2] = convolvedValue;
                }
            }

            // Copy the temporary (convolved) bitmap onto the processed bitmap:
            for (int i = halfSizeY; i < processedBitmap.Height - halfSizeY; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                byte* cRow = (byte*)convolvedData.Scan0 + (i * convolvedData.Stride);
                for (int j = halfSizeX; j < processedBitmap.Width - halfSizeX; j++)
                {
                        pRow[j * PIXEL_SIZE] = cRow[j * PIXEL_SIZE];
                        pRow[j * PIXEL_SIZE + 1] = cRow[j * PIXEL_SIZE + 1];
                        pRow[j * PIXEL_SIZE + 2] = cRow[j * PIXEL_SIZE + 2];
                }
            }
            convolvedBitmap.UnlockBits(convolvedData);
        }

        public Bitmap GetSkinPixelsRGB()
        {
            Bitmap skinBitmap = new Bitmap(processedBitmap.Width, processedBitmap.Height);
            BitmapData skinData = skinBitmap.LockBits(
                   new Rectangle(0, 0, processedBitmap.Width, processedBitmap.Height),
                   ImageLockMode.ReadWrite, PixelFormat.Format24bppRgb);
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride); // YCbCr format
                byte* sRow = (byte*)skinData.Scan0 + (i * skinData.Stride); // Y component as RGB
                for (int j = 0; j < processedData.Width; j++)
                {
                    byte b = pRow[j * PIXEL_SIZE];
                    byte g = pRow[j * PIXEL_SIZE + 1];
                    byte r = pRow[j * PIXEL_SIZE + 2];

                    byte maxColor = Math.Max(r, Math.Max(g, b));
                    byte minColor = Math.Min(r, Math.Min(g, b));

                    if ( ((r > 95) && (g > 40) && (b > 20)) & ((maxColor-minColor) > 15) &&
                         ((int)Math.Abs(r-g) > 15) && ((int)Math.Abs(r-g) < 75) && (r > g) && (r > b))
                    {
                        sRow[j * PIXEL_SIZE] = 255;
                        sRow[j * PIXEL_SIZE + 1] = 255;
                        sRow[j * PIXEL_SIZE + 2] = 255;
                    }
                    else
                    {
                        sRow[j * PIXEL_SIZE] = 0;
                        sRow[j * PIXEL_SIZE + 1] = 0;
                        sRow[j * PIXEL_SIZE + 2] = 0;
                    }
                }
            }
            skinBitmap.UnlockBits(skinData);
            return skinBitmap;
        }

        #endregion

        #region Public New methods
        public void EmptyGrayImage()
        {
            if (!isGrayScale)
            {
                ConvertToStandardGrayScale();
            }
            for (int i = 0; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 0; j < processedData.Width; j++)
                {
                    byte pixelColor = (byte)(pRow[j * PIXEL_SIZE]);
                    pixelColor = 0;
                    pRow[j * PIXEL_SIZE] = pixelColor;
                    pRow[j * PIXEL_SIZE + 1] = pixelColor;
                    pRow[j * PIXEL_SIZE + 2] = pixelColor;
                }
            }
        }

        // Preferably, remove single pixels before calling.
        public int ConnectedComponents()
        {
            if (!isGrayScale)
            {
                ConvertToStandardGrayScale();
            }
            if (!isBinarized)
            {
                BinarizeGray(DEFAULT_THRESHOLD1, DEFAULT_THRESHOLD2);
            }

            // Three-pass algorithm. Variant of: http://en.wikipedia.org/wiki/Connected_Component_Labeling#Two-pass
            // But one pass is introduced so that we may have a fixed-sized array to represent DisjointSets.

            int[,] labels = new int[this.ProcessedBitmap.Height, this.ProcessedBitmap.Width];
            labels.Initialize(); // Init to 0 (background)
            int nrOfLabels = 1; //Background is label 0

            // First pass
            // Label all pixels. Count number of labels.
            #region First pass
            // First element
            byte* row0 = (byte*)processedData.Scan0;
            if (row0[0] != 0)
            {
                labels[0, 0] = nrOfLabels;
                nrOfLabels++;
            }
            // First Row:
            for (int j = 1; j < processedData.Width; j++)
            {
                byte pixelColor = (byte)(row0[j * PIXEL_SIZE]);
                if (pixelColor != 0 && labels[0, (j - 1)] != 0)
                {
                    labels[0, j] = labels[0, (j - 1)];
                }
                else if (pixelColor != 0)
                {
                    labels[0, j] = nrOfLabels;
                    nrOfLabels++;
                }
            }

            //First Column:
            for (int i = 1; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                byte pixelColor = (byte)(pRow[0]);
                if (pixelColor != 0 && labels[(i - 1), 0] != 0)
                {
                    labels[i, 0] = labels[(i - 1), 0];
                }
                else if (pixelColor != 0)
                {
                    labels[i, 0] = nrOfLabels;
                    nrOfLabels++;
                }
            }

            //The rest:
            for (int i = 1; i < processedBitmap.Height; i++)
            {
                byte* pRow = (byte*)processedData.Scan0 + (i * processedData.Stride);
                for (int j = 1; j < processedData.Width; j++)
                {
                    byte pixelColor = (byte)(pRow[j * PIXEL_SIZE]);
                    int westLabel = labels[i, j - 1];
                    int northLabel = labels[i - 1, j];

                    if (pixelColor != 0 && westLabel != 0)
                    {
                        labels[i, j] = westLabel;
                    }
                    else if (pixelColor != 0 && northLabel != 0)
                    {
                        labels[i, j] = northLabel;
                    }
                    else if (pixelColor != 0)
                    {
                        labels[i, j] = nrOfLabels;
                        nrOfLabels++;
                    }
                }
            }
            #endregion

            // Second pass
            // Create DisjointSets with same size as number of lables. Union all connected sets.
            #region Second pass
            DisjointSets disjointSets = new DisjointSets(nrOfLabels); //Label 0 is backgound

            for (int i = 1; i < processedBitmap.Height; i++)
            {
                for (int j = 1; j < processedData.Width; j++)
                {
                    int currentLabel = labels[i, j];
                    int westLabel = labels[i, j - 1];
                    int northLabel = labels[i - 1, j];

                    if (currentLabel != 0 && westLabel != 0)
                    {
                        disjointSets.Union(currentLabel, westLabel);
                    }
                    if (currentLabel != 0 && northLabel != 0)
                    {
                        disjointSets.Union(currentLabel, northLabel);
                    }
                }
            }
            #endregion

            // Third pass
            // Relabel all. Count Region size.
            #region Third pass
            int[] regionSizeByLabel = new int[nrOfLabels];
            regionSizeByLabel.Initialize();

            for (int i = 0; i < processedBitmap.Height; i++)
            {
                for (int j = 0; j < processedData.Width; j++)
                {
                    int currentLabel = labels[i, j];
                    if (currentLabel != 0)
                    {
                        int jointSetLabel = disjointSets.Find(currentLabel);
                        regionSizeByLabel[jointSetLabel]++;
                        labels[i, j] = jointSetLabel;
                    }
                }
            }
            #endregion

            // Find size of largest Region
            #region Size
            int sizeOfLargestRegion = 0;
            int labelOfLargestRegion = 0;
            for (int i = 0; i < regionSizeByLabel.Length; i++)
            {
                if (regionSizeByLabel[i] > sizeOfLargestRegion)
                {
                    sizeOfLargestRegion = regionSizeByLabel[i];
                    labelOfLargestRegion = i;
                }
            }
            #endregion

            // Find "middle" of largest Region
            #region Middle
            int leftMostPixelOfLargestRegion = 0;
            int rightMostPixelOfLargestRegion = 0;
            for (int j = 0; j < processedData.Width; j++)
            {
                for (int i = 0; i < processedData.Height; i++)
                {
                    if (labels[i, j] == labelOfLargestRegion && leftMostPixelOfLargestRegion == 0)
                    {
                        leftMostPixelOfLargestRegion = j;
                        rightMostPixelOfLargestRegion = j;
                        break;
                    }
                    else if (labels[i, j] == labelOfLargestRegion)
                    {
                        rightMostPixelOfLargestRegion = j;
                        break;
                    }
                }
            }

            int middle = leftMostPixelOfLargestRegion + 
                ((rightMostPixelOfLargestRegion - leftMostPixelOfLargestRegion) / 2);
            int middleOfLargestRegionInCameraRegion = 0;
            if (middle < processedData.Width / 3)
            {
                middleOfLargestRegionInCameraRegion = 15; //Left
            }
            else if (middle > (processedData.Width / 3) * 2)
            {
                middleOfLargestRegionInCameraRegion = -15; //Right
            }
            else
            {
                middleOfLargestRegionInCameraRegion = 0; //Middle
            }
            #endregion

            this.sizeOfLargestComponent = sizeOfLargestRegion;
            this.labelOfLargestComponent = labelOfLargestRegion;
            this.middleOfLargestComponent = middleOfLargestRegionInCameraRegion;
            this.pixelMiddleOfLargestComponent = middle;
            this.connectedComponentsLabels = labels;
            this.isComponentsConnected = true;

            return sizeOfLargestRegion;
        }

        //Asumes locked input and forground and background of equal size
        public static ImageProcessor FrameDiffRunningAverage(ImageProcessor forground, ImageProcessor background)
        {
            ImageProcessor diff = new ImageProcessor(new Bitmap(background.ProcessedBitmap.Width, background.ProcessedBitmap.Height));
            diff.Lock();

            //(7.24) + (7.25)
            for (int i = 0; i < diff.processedBitmap.Height; i++)
            {
                byte* pRowDiff = (byte*)diff.processedData.Scan0 + (i * diff.processedData.Stride);
                byte* pRowBackground = (byte*)background.processedData.Scan0 + (i * background.processedData.Stride);
                byte* pRowForground = (byte*)forground.processedData.Scan0 + (i * forground.processedData.Stride);
                for (int j = 0; j < diff.processedData.Width; j++)
                {
                    //(7.24)
                    int pixelColorInt = pRowForground[j * PIXEL_SIZE] - pRowBackground[j * PIXEL_SIZE];
                    byte pixelColorDiff;

                    if (pixelColorInt >= 0)
                    {
                        pixelColorDiff = (byte)pixelColorInt;
                    }
                    else
                    {
                        pixelColorDiff = (byte)(-pixelColorInt);
                    }

                    pRowDiff[j * PIXEL_SIZE] = pixelColorDiff;
                    pRowDiff[j * PIXEL_SIZE + 1] = pixelColorDiff;
                    pRowDiff[j * PIXEL_SIZE + 2] = pixelColorDiff;

                    //(7.25)
                    byte pixelColorBackground;
                    pixelColorBackground = (byte)((RUNNING_AVERAGE_BETA * pRowForground[j * PIXEL_SIZE]
                        + (10 - RUNNING_AVERAGE_BETA) * pRowBackground[j * PIXEL_SIZE]) / 10);

                    pRowBackground[j * PIXEL_SIZE] = pixelColorBackground;
                    pRowBackground[j * PIXEL_SIZE + 1] = pixelColorBackground;
                    pRowBackground[j * PIXEL_SIZE + 2] = pixelColorBackground;
                }
            }

            return diff;
        }
        #endregion

        #region Public properties
        public int MiddleOfLargestComponent
        {
            get
            {
                if (this.isComponentsConnected)
                {
                    return this.middleOfLargestComponent;
                }
                else
                {
                    return 0;
                }
            }
        }

        public int PixelMiddleOfLargestComponent
        {
            get
            {
                if (this.isComponentsConnected)
                {
                    return this.pixelMiddleOfLargestComponent;
                }
                else
                {
                    return 0;
                }
            }
        }

        public Bitmap ProcessedBitmap
        {
            get { return processedBitmap; }
        }

        public int[,] IntegralImage
        {
            get { return integralImage; }
            set { integralImage = value; }
        }
        #endregion

    }
}
